﻿using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using System.Windows.Forms;
using MotionPlanning.MotionPlanningPrimitives;
using MotionPlanning.MotionPlanningAlgorithms;
using MotionPlanning.MotionPlanningComponents.MotionPlanningSystems;
using MotionPlanning.MotionPlanningComponents.MotionPlanningSystems.CollisionPrimitives;

namespace MotionPlanning.MotionPlanningComponents.Visualizations
{
    class Visualizer_SimplifiedPlanarPhysicalRobots : Visualizer
    {
        Rectangle envRect;
        SolidBrush brushEnvClear;
        SolidBrush brushNodePlot;
        SolidBrush brushStateRed;
        SolidBrush brushObstacle;
        protected SolidBrush brushRobot;
        SolidBrush brushRoot;
        SolidBrush brushGoal;
        protected Pen edgeWhite;
        Pen edgeBlue;
        Pen edgeRed;
        Pen obsColor;
        Pen rbtColor;
        private double canvasScreenRate = 0.9;
        
        public Visualizer_SimplifiedPlanarPhysicalRobots(Graphics canvas, Rectangle rect)
            : base(canvas, rect)
        {
            this.canvas = canvas;
            envRect = rect;
            canvasRect = new Rectangle(0, 0, envRect.Width, envRect.Height);

            brushCanvasClear = new SolidBrush(Color.Gray);
            brushEnvClear = new SolidBrush(Color.White);
            brushNodePlot = new SolidBrush(Color.Red);
            canvasAxisGray = new Pen(Color.Gray);
            edgeBlue = new Pen(Color.Blue, 2);
            edgeRed = new Pen(Color.Red, 2);
            edgeWhite = new Pen(Color.White, 1);
            brushStateRed = new SolidBrush(Color.Red);
            brushObstacle = new SolidBrush(Color.FromArgb(180, Color.Red));
            brushRobot = new SolidBrush(Color.FromArgb(200, Color.Black));
            brushRoot = new SolidBrush(Color.FromArgb(200, Color.Green));
            brushGoal = new SolidBrush(Color.FromArgb(200, Color.Blue));
            calibrater_x = canvasRect.Width / 2;
            calibrater_y = -canvasRect.Height / 2;
            center_x = envRect.Width / 2;
            center_y = envRect.Height / 2;
            screen_width = envRect.Width;
            screen_height = envRect.Height;
            obsColor = new Pen(Color.DarkBlue);
            rbtColor = new Pen(Color.Red);

        }

        override public void RenderFrame()
        {
            ClearCanvas();
            DrawPhysicalEnvironment((System_SimplifiedPlanarPhysicalRobot)planner_subject.GetSystem());
            DrawPhysicalRobot((System_SimplifiedPlanarPhysicalRobot)planner_subject.GetSystem(), planner_subject.GetRoot().get_Value().get_q());

            if (planner_subject.GetSolution() == null)
                this.PlotVerticesWithCost(planner_subject.GetVertices().GetAllDataObjects());
            else
                this.PlotTrajectory(planner_subject.GetSolution());

            canvas.DrawLine(canvasAxisGray, 0, center_y, screen_width, center_y);
            canvas.DrawLine(canvasAxisGray, center_x, 0, center_x, screen_height);
        }

        override public void PlotEdges(List<object> train)
        {
            for (int i = 0; i < train.Count; i += 1)
            {
                List<Edge> pe = ((Node)train[i]).get_Parents();
                if (pe == null) continue;
                Edge p = pe[pe.Count - 1];
                double[] q1 = p.get_entry().get_Value().get_q();
                double[] q2 = p.get_exit().get_Value().get_q();

                int screen_x1 = (int)(q1[0] * this.calibrater_x + center_x);
                int screen_y1 = (int)(q1[1] * this.calibrater_y + center_y);

                int screen_x2 = (int)(q2[0] * this.calibrater_x + center_x);
                int screen_y2 = (int)(q2[1] * this.calibrater_y + center_y);

                canvas.DrawLine(edgeBlue, screen_x1, screen_y1, screen_x2, screen_y2);
                this.DrawPhysicalRobot((System_SimplifiedPlanarPhysicalRobot)planner_subject.GetSystem(), q2);
            }
        }

        override public void PlotVertices(List<object> train)
        {
            throw new NotImplementedException();
        }

        override public void PlotTrajectory(Trajectory trajectory)
        {
            if (trajectory == null)
                return;
            List<Node> E = trajectory.GetNodes();
            List<object> train = new List<object>(E.Count);
            for (int i = 0; i < E.Count; ++i)
                train.Add(E[i]);
            PlotEdges(train);
        }

        override public void SetHostPlanner(Planner_RRTBased host)
        {
            planner_subject = host;
            double[] q_max = planner_subject.GetSystem().Get_q_max();
            double[] q_min = planner_subject.GetSystem().Get_q_min();
            double range_x = (Math.Abs(q_max[0]) > Math.Abs(q_min[0])) ? Math.Abs(q_max[0]) : Math.Abs(q_min[0]);
            double range_y = (Math.Abs(q_max[1]) > Math.Abs(q_min[1])) ? Math.Abs(q_max[1]) : Math.Abs(q_min[1]);
            double largestRange = Math.Abs(range_x);
            if (Math.Abs(range_y) > largestRange)
                largestRange = Math.Abs(range_y);
            this.calibrater_x = canvasRect.Width / 2 / largestRange * canvasScreenRate;
            this.calibrater_y = -canvasRect.Height / 2 / largestRange * canvasScreenRate;

            envRect = new Rectangle(
                (int)(center_x + q_min[0] * calibrater_x),
                (int)(center_y + q_max[1] * calibrater_y),
                (int)(range_x * 2 * calibrater_x),
                (int)(range_y * -2 * calibrater_y));
        }

        override public void ClearCanvas()
        {
            canvas.FillRectangle(brushCanvasClear, canvasRect);
            canvas.FillRectangle(brushEnvClear, envRect);
        }

        public void DrawPhysicalEnvironment(System_SimplifiedPlanarPhysicalRobot env)
        {
            List<Polygon_2D> obsList = env.Get_Geometry_Environment();
            for (int i = 0; i < obsList.Count; ++i)
            {
                List<double[]> current_obs = null;
                obsList[i].TransformToEnvironment(null, ref current_obs);
                Point[] points = new Point[current_obs.Count];
                for (int j = 0; j < current_obs.Count; ++j)
                {
                    points[j].X = (int)(center_x + current_obs[j][0] * calibrater_x);
                    points[j].Y = (int)(center_y + current_obs[j][1] * calibrater_y);
                }

                canvas.FillPolygon(brushObstacle, points);
            }
        }

        virtual public void DrawPhysicalRobot(System_SimplifiedPlanarPhysicalRobot env, double[] q)
        {
            List<Polygon_2D> objList = env.Get_Geometry_Robot();
            for (int i = 0; i < objList.Count; ++i)
            {
                List<double[]> current_obs = null;
                objList[i].TransformToEnvironment(q, ref current_obs);
                Point[] points = new Point[current_obs.Count];
                for (int j = 0; j < current_obs.Count; ++j)
                {
                    points[j].X = (int)(center_x + current_obs[j][0] * calibrater_x);
                    points[j].Y = (int)(center_y + current_obs[j][1] * calibrater_y);
                }

                canvas.FillPolygon(brushRobot, points);
                canvas.DrawLine(edgeWhite, points[0], points[1]);
                canvas.DrawLine(edgeWhite, points[1], points[2]);
                canvas.DrawLine(edgeWhite, points[2], points[3]);
                canvas.DrawLine(edgeWhite, points[3], points[0]);

                double robotCenter_x_left = (current_obs[0][0] + current_obs[1][0]) / 2;
                double robotCenter_y_left = (current_obs[0][1] + current_obs[1][1]) / 2;
                double robotCenter_x_top = (current_obs[0][0] + current_obs[3][0]) / 2;
                double robotCenter_y_top = (current_obs[0][1] + current_obs[3][1]) / 2;
                double robotCenter_x_bottom = (current_obs[1][0] + current_obs[2][0]) / 2;
                double robotCenter_y_bottom = (current_obs[1][1] + current_obs[2][1]) / 2;
                double robotCenter_x_center = (robotCenter_x_bottom + robotCenter_x_top) / 2;
                double robotCenter_y_center = (robotCenter_y_bottom + robotCenter_y_top) / 2;

                double robotCenter_x = robotCenter_x_left * 0.3333 + robotCenter_x_center * 0.6666;
                double robotCenter_y = robotCenter_y_left * 0.3333 + robotCenter_y_center * 0.6666;
                int robotCenter_x_screen = (int)(center_x + robotCenter_x * calibrater_x);
                int robotCenter_y_screen = (int)(center_y + robotCenter_y * calibrater_y);
                canvas.FillEllipse(brushRed, robotCenter_x_screen - 3, robotCenter_y_screen - 3, 6, 6);
            }

        }
    }
}
